#include "main.h" // 所有引脚信息更改在main.h里改宏

// *************************** 例程硬件连接说明 ***************************
/*

            连接好电机、主板、驱动板、摄像头、编码器
            (请确保全部正确连接后在使用此例程，所有外设均会使用)
*/
// *************************** 例程测试说明 ***************************
/*
            本例程包含功能：
            1.初始化摄像头并采集图像进行大津法二值化
            2.初始化屏幕并显示关键信息
            3.根据偏差计算最基本的舵机打角
            4.运用最基本pid参数和pid算法使电机旋转 (pid参数在pid.c中)


            注意！注意！注意！注意！
            注意！注意！注意！注意！
            注意！注意！注意！注意！

            1.请仔细阅读main.h的所有注释，确保所有宏都和自己的硬件连接对应
            2.若屏幕显示二值化后的图像有缺陷，可能不能正常走直线，
            可以自行更改二值化算法或找光环境良好且均匀的环境测试例程（例程不提供更多的二值化算法）

*/

// **************************** 代码区域 ****************************

// 舵机动作角度中值（不能写死，因为每个硬件都不一样）
const float servo_motor_duty_middle = (SERVO_MOTOR_R_MAX + SERVO_MOTOR_L_MAX) / 2.f; //.f代表2是一个浮点数

void Init()
{

    // 初始化flash, 储存参数. 一个扇区有8页, 一页可以储存4096字节, 一个参数占4个字节, 因此一页最多只能存64个参数
    flash_init();
		//初始化屏幕	（得换，我买的不是这个屏幕）
    // tft180_set_dir(TFT180_CROSSWISE);                                           // 需要先横屏 不然显示不下
    tft180_init();

    // mt9v03x摄像头初始化（循环初始化摄像头，直到成功为止，增强系统稳定性。）
    while (1)
    {
        if (mt9v03x_init())
            tft180_show_string(0, 16, "mt9v03x reinit.");
        else
            break;
        system_delay_ms(1000);
    }
    tft180_show_string(0, 16, "init success.");

    // 编码器初始化
    encoder_quad_init(ENCODER_1, ENCODER_1_A, ENCODER_1_B); // 初始化编码器模块与引脚 正交解码编码器模式
    encoder_quad_init(ENCODER_2, ENCODER_2_A, ENCODER_2_B); // 初始化编码器模块与引脚 正交解码编码器模式

    // 电机初始化
    pwm_init(MOTOR1_PWM, 17000, 0); // PWM 通道初始化频率 17KHz 占空比初始为 0
    gpio_init(MOTOR1_DIR, GPO, GPIO_HIGH, GPO_PUSH_PULL); // GPIO 初始化为输出 默认上拉输出高
    pwm_init(MOTOR2_PWM, 17000, 0); // PWM 通道初始化频率 17KHz 占空比初始为 0
    gpio_init(MOTOR2_DIR, GPO, GPIO_HIGH, GPO_PUSH_PULL); // GPIO 初始化为输出 默认上拉输出高
		//引脚名，引脚模式（out），初始输出高电平，输出模式为“推挽”（是最常用的，除此之外还有“开漏”，我是没看懂啥意思，可以自己查）
    // 舵机初始化
    pwm_init(SERVO_MOTOR_PWM, SERVO_MOTOR_FREQ, 0);

    // PID初始化（此函数在pid.c中）
    My_Pid_Init();

    // 定时器0初始化，10ms可调：这是速度环的心脏。初始化周期中断定时器0，并设置它每隔 10毫秒 就触发一次中断。PIT_IRQHandler 函数将会因此被周期性地调用。
    pit_ms_init(PIT_CH0, 10);
}
//一些全局变量
uint8_t image[MT9V03X_H][MT9V03X_W];//二维数组，用于存储从mt9v03x_image复制的图像数据，尺寸由宏定义
short hist_gram[256];//直方图数组，数值表示图像中灰度为“i”的像素点有多少个，是大津法的输入
uint8_t left_line[MT9V03X_H];//
uint8_t mid_line[MT9V03X_H];//
uint8_t right_line[MT9V03X_H];//三个数组，用来存储赛道边界和中心线的坐标，代表坐标在第i行上的x坐标

int main(void)
{
    clock_init(SYSTEM_CLOCK_600M); // 不可删除//设置CPU的核心频率为600MHz
    debug_init();                  // 调试端口初始化
    system_delay_ms(300);          // 等待主板其他外设上电完成

	Init();//总初始化函数，调用上面的所有硬件初始化
    interrupt_global_enable(0);//打开中断的总开关，接下来才能触发各种中断

    while (1)//死循环
    {
        //  mt9v03x摄像头
        if (mt9v03x_finish_flag)//循环内唯一入口，flag是一个标志位，当摄像头硬件成功采集完一帧完整的图像并存入它的内部缓冲区后，库会自动将这个标志位设置为 1
        {												//这种模式被称为事件驱动，它确保了我们处理的每一帧图像都是最新的、完整的，大大提高了程序的效率和稳定性。
            // 另寻空间将图像保存下来，以免产生因读写冲突带来的未知后果
            memcpy((uint8_t *)image, (uint8_t *)mt9v03x_image, sizeof(uint8_t) * MT9V03X_H * MT9V03X_W);
            // 获取直方图，输入是我们的图像 image，输出是填充好的直方图数组 hist_gram
            get_hist_gram((uint8_t *)image, MT9V03X_H, MT9V03X_W, hist_gram);
						// 计算大津法阈值，输入是直方图 hist_gram，输出是一个计算出的最佳二值化阈值 threshold
            unsigned char threshold = get_threshold_otsu(hist_gram);
            // 二值化处理，输入是原始图像 image 和阈值 threshold，它会直接修改 image 数组，把里面的灰度图变成只有0（黑）和255（白）的二值图。
            binaryzation_process((uint8_t *)image, MT9V03X_H, MT9V03X_W, threshold);
            // 边界线寻找，输入是二值化后的图像 image，输出是三个被填充好数据的边界线数组 left_line, mid_line, right_line。
            auxiliary_process((uint8_t *)image, MT9V03X_H, MT9V03X_W, threshold, left_line, mid_line, right_line);
						//[调试] 在二值化图像上把找到的边界线画成黑色，方便观察算法效果。
            for (uint8_t _i = 0; _i < MT9V03X_H; ++_i)
            {
                // 将边界线也显示出来
                image[_i][left_line[_i]] = 0;
                image[_i][mid_line[_i]] = 0;
                image[_i][right_line[_i]] = 0;
            }
            // 显示图像
            tft180_displayimage03x((uint8_t *)image, 125, 100);

            // 取图像下1/4处做误差判断，mid_err = 图像中心X - 赛道中心X：横向偏差，大于0则偏左
            float mid_err = (MT9V03X_W/2) - mid_line[MT9V03X_H-MT9V03X_H/4];
            // 舵机根据误差乘以系数打角，这是p控制器（比例控制），0.5称为p参数，p越大方向越大，SERVO_DIR是舵机方向系数（1或-1，防止装反）
            float tmp_duty = servo_motor_duty_middle - mid_err * 0.5 * SERVO_DIR;
            // 限幅：强制“夹”在舵机能正常工作的范围内，防止计算出一个异常值（比如因为 mid_err 巨大）而让舵机堵转、损坏。
            tmp_duty = MAX(tmp_duty,MIN(SERVO_MOTOR_L_MAX,SERVO_MOTOR_R_MAX));
            tmp_duty = MIN(tmp_duty,MAX(SERVO_MOTOR_L_MAX,SERVO_MOTOR_R_MAX));
						//将经过计算和限幅的最终占空比，通过PWM通道发送给舵机，完成一次转向控制。SERVO_MOTOR_DUTY()可能是一个宏，用来做单位转换。
            pwm_set_duty(SERVO_MOTOR_PWM, SERVO_MOTOR_DUTY(tmp_duty));

            // 显示关键信息
            tft180_show_int(0, 0, encoder_data_1, 3);//编码器读数
            tft180_show_int(0, 30, encoder_data_2, 3);//编码器读数
            tft180_show_float(0, 50, speed_pwm, 4, 2);//电机pwm值

            // 处理完一帧图像后务必把该标志位清零！此帧图像处理完成，进入待命，等待下一帧图像
            mt9v03x_finish_flag = 0;
        }
    }
}
